
#include "urcontroldepthcameraa.hpp"

#include "/home/u/ur3/urRemote/serverbase.hpp"

#include "/home/u/number/t3/doublebf2.hpp"


class DepthCameraServer
        : public ServerBase
        , public URControlDepthCameraA
{
public:

    int sockfdout;

    DoubleBf2<cv::Mat> xyzBuffer;

    int pind[8];

    std::mutex mtx;

    DepthCameraServer()
        : ServerBase()
        , URControlDepthCameraA()
    {
        for(int i=0;i<8;i++) pind[i]=0;
    }



    void ArmMove(cv::Vec2d cornerPixel[4])
    {
        if(mtx.try_lock())
        {


            char ipUR[]="192.168.0.200";
            char ipPC[]="192.168.0.202";

            double openmm=110;

            double a=0.2;
            double v=0.4;

            std::vector<cv::Point> vtx(4);
            for(int i=0;i<4;i++)
            {
                vtx[i].x=cornerPixel[i][0];
                vtx[i].y=cornerPixel[i][1];
            }

            cv::RotatedRect rr=cv::minAreaRect(vtx);

            cv::Vec4d planeAtBase(0.0131388, 0.021942, -0.999673, 0.0213691);

            cv::Vec6d fingerToHead(0,0,0.21,0,0,CV_PI/4);

            cv::Mat *pxyz=NULL;
            xyzBuffer.Read(pxyz);

            bool res=PickBox(openmm, planeAtBase, fingerToHead, *pxyz, rr, ipUR, ipPC, a, v);

            delete pxyz;

            if(res)
            {



                ToST tst;

                tst.StartUpdate();

                std::this_thread::sleep_for(std::chrono::seconds(1));

#if 1

                std::cout<<"send go\n"<<std::flush;
                unsigned char num='5';
                tst.SendMsgUChar(&num,1);
                while(!tst.IsAGVStop()){ std::this_thread::yield(); }
                //        tst.EndUpdate();
#endif



                cv::Vec6d urp;

                urp=cv::Vec6d(-0.518745,-0.0684919,-0.0381951,1.26058,-2.86865,-0.0722171);

                URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5],a,v);

                URControl::RG2(ipUR, ipPC, openmm);

                urp=cv::Vec6d(-0.151424,-0.270796,0.43873,-1.30697,2.84104,-0.01504);

                URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5],a,v);

                URControl::RG2(ipUR, ipPC, 0);
#if 0
                while(k!='g')
                {
                    std::this_thread::yield();
                }
                k=0;
#else
                std::this_thread::sleep_for(std::chrono::seconds(1));
#endif

#if 1
                num='8';
                tst.SendMsgUChar(&num,1);
                while(!tst.IsAGVStop2()){ std::this_thread::yield(); }
#endif


#if 0
                while(k!='g')
                {
                    std::this_thread::yield();
                }
                k=0;
#else
                std::this_thread::sleep_for(std::chrono::seconds(1));
#endif

#if 1
                num='2';
                tst.SendMsgUChar(&num,1);
                while(!tst.IsAGVStop3()){ std::this_thread::yield(); }
#endif


                tst.EndUpdate();




                urp=cv::Vec6d(-0.283063,-0.0766516,0.43017,0.591763,-1.51612,-0.683699);

                URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5],a,v);

                std::this_thread::sleep_for(std::chrono::seconds(4));
            }
            else {
                std::cout<<"nothing pick\n"<<std::flush;
            }
            mtx.unlock();

            std::cout<<"end move\n"<<std::flush;



        }

        delete []cornerPixel;

        std::cout<<"end thread\n"<<std::flush;

    }

    static void ArmMoveThread(DepthCameraServer *pData, cv::Vec2d cornerPixel[4])
    {
        pData->ArmMove(cornerPixel);
    }


    void Receive(uint16_t portno=8765)
    {
        int sockfd=ServerStart(portno);

        int n;
        char buffer[256];

        std::vector<std::thread> ths;


        while(k!=27)
        {
            bzero(buffer,256);
            n=ServerReceive(sockfd, buffer, 255);
            int *pindex=(int*)buffer;

            if(pindex[0]>0 && pindex[1]>0 &&
                    pindex[2]>0 && pindex[3]>0 &&
                    pindex[4]>0 && pindex[5]>0 &&
                    pindex[6]>0 && pindex[7]>0 )
            {

                memcpy(pind, pindex, n);



                cv::Vec2d *corner=new cv::Vec2d[4];
                corner[0][0]=pindex[0];
                corner[0][1]=pindex[1];
                corner[1][0]=pindex[2];
                corner[1][1]=pindex[3];
                corner[2][0]=pindex[4];
                corner[2][1]=pindex[5];
                corner[3][0]=pindex[6];
                corner[3][1]=pindex[7];

                if(ths.size()>10000)
                {
                    std::cout<<"end first thread\n"<<std::flush;

                    if(ths[0].joinable())
                        ths[0].join();
                    ths.erase(ths.begin());
                }

                std::cout<<"add thread\n"<<std::flush;
                                            ths.push_back(std::thread(ArmMoveThread, this, corner));


                std::cout<<"end add thread\n"<<std::flush;

            }
        }

        close(sockfd);

        for(auto &th:ths) th.join();
    }

    static void ReceiveThread(DepthCameraServer *pData)
    {
        pData->Receive();
    }

    void startserver(uint16_t portno=2345)
    {

        k=0;

        sockfdout=ServerStart(portno);

        std::thread th1(ReceiveThread, this);

        Start();

        close(sockfdout);

        th1.join();
    }


    virtual void ProcessFrame(const cv::Mat &color, const cv::Mat &ir, const cv::Mat &depth, const cv::Mat &xyz, const cv::Mat &texture, const cv::Mat &aligned_depth, const cv::Mat &aligned_xyz)
    {
        cv::Mat *pm=new cv::Mat();
        aligned_xyz.copyTo(*pm);
        xyzBuffer.Write(pm);

//        std::cout<<"encode frame\n"<<std::flush;

        std::vector<unsigned char> mec;
        cv::imencode(".jpg", color, mec);

//        std::cout<<"end encode frame\n"<<std::flush;
        int n=ServerSend(sockfdout, mec.data(), mec.size());

//        std::cout<<"show img\n"<<std::flush;

        cv::line(color, cv::Point(pind[0],pind[1]), cv::Point(pind[2],pind[3]), cv::Scalar(0,0,255), 3 );
        cv::line(color, cv::Point(pind[2],pind[3]), cv::Point(pind[4],pind[5]), cv::Scalar(0,255,0), 3 );
        cv::line(color, cv::Point(pind[4],pind[5]), cv::Point(pind[6],pind[7]), cv::Scalar(255,0,0), 3 );
        cv::line(color, cv::Point(pind[6],pind[7]), cv::Point(pind[0],pind[1]), cv::Scalar(255,0,255), 3 );

        cv::imshow("m",color);
        k=cv::waitKey(1);
    }


};


int main()
{

//    URControlDepthCameraA a;
    //    a.GetOne();
//    a.Start();

    DepthCameraServer a;
    a.startserver();


    cv::waitKey(100000);


//URControl::test();


    return 0;
}
